#include "ctrl_thread.h"

#include "motor.h"
#include "main.h"
#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"

#include "usart.h"
#include "sbus.h"

#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"


extern uint16_t rc_data[16];

int16_t m_speed[2];
int16_t m_speed_pid[2];
extern int16_t motor_speed[2];

TimerHandle_t xTimerMotor;


static void vTimerCallback(TimerHandle_t xTimer)
{
	motor_ctrl_loop();
}

void StartCtrlTask(void const *argument)
{
	/* USER CODE BEGIN StartDefaultTask */

    sbus_init();
    motor_init();
    
	xTimerMotor = xTimerCreate("MOTOR Timer",
							   10,
							   pdTRUE,
							   (void *)0,
							   vTimerCallback);

	if (xTimerMotor != NULL)
	{
		xTimerStart(xTimerMotor, 0);
	}

	/* Infinite loop */
	for (;;)
	{
		osDelay(4000);
	}
	/* USER CODE END StartDefaultTask */
}

void motor_ctrl_loop(void)
{
    static int8_t speed_base,speed0,speed1,speed_diff;
      
    speed_base = (rc_data[1]-1000)/5;
    speed_diff = (rc_data[0]-1000)/10;
    m_speed[0] = speed_base + speed_diff;
    m_speed[1] = speed_base - speed_diff;
      
//    if(m_speed[0]<3 &&   m_speed[0]>-3)
//        m_speed[0]=0;
//    
//    if(m_speed[1]<3 && m_speed[1]>-3)
//        m_speed[1]=0;
    
    //pid_ctrl(m_speed[0], motor_speed[0], &m_speed_pid[0], 0);
    //pid_ctrl(m_speed[1], motor_speed[1], &m_speed_pid[1], 1);
    //motor_ctrl(m_speed_pid[0],m_speed_pid[1]);
    
    motor_ctrl(m_speed[0],m_speed[1]);
    
    //printf("%d\t%d\t%d\r\n", m_speed[0],motor_speed[0],m_speed_pid[0]);
}
